#include "Process.h"
using namespace std;
using namespace cv;

void ValueSum(cv::Mat& img,uint64& ResVal)
{
    static int A = 32;//AVX2
    assert(img.cols >= A);
    size_t bodyWidth = AlignLo(img.cols, A);
    bool EndType = bodyWidth != img.cols;
    size_t Signm = 0;

    __m256i tailMask;
    if (EndType)
    {
        int position = A - img.cols + bodyWidth;
        static uchar P[32] = { 0 };
        for (size_t i = 0; i < position; ++i)
            P[i] = 0;
        for (size_t i = position; i < 32; ++i)
            P[i] = 0xFF;
        tailMask = _mm256_loadu_si256((__m256i*)P);
        Signm = img.cols - A;
    }
    uchar* Ptr;
    __m256i fullSum = _mm256_setzero_si256();
    for (size_t row = 0; row < img.rows; ++row)
    {
        Ptr = img.ptr<uchar>(row);
        for (size_t col = 0; col < bodyWidth; col += A)
        {
            const __m256i src_ = _mm256_loadu_si256((__m256i*)(Ptr + col));
            fullSum = _mm256_add_epi64(_mm256_sad_epu8(src_, K_ZERO), fullSum);
        }
        if (EndType)
        {
            const __m256i src_ = _mm256_and_si256(tailMask, _mm256_loadu_si256((__m256i*)(Ptr + Signm)));
            fullSum = _mm256_add_epi64(_mm256_sad_epu8(src_, K_ZERO), fullSum);
        }
    }
    ResVal = 0;
    for (size_t i = 0; i < 4; i++)
    {
        ResVal += fullSum.m256i_i64[i];
    }
}

void SquareSum(cv::Mat& img, uint64& ResVal)
{
    static int A = 32;//AVX2
    assert(img.cols >= A);
    size_t bodyWidth = AlignLo(img.cols, A);
    bool EndType = bodyWidth != img.cols;
    size_t Signm = 0;

    __m256i tailMask;
    if (EndType)
    {
        int position = A - img.cols + bodyWidth;
        static uchar P[32] = { 0 };
        for (size_t i = 0; i < position; ++i)
            P[i] = 0;
        for (size_t i = position; i < 32; ++i)
            P[i] = 0xFF;
        tailMask = _mm256_loadu_si256((__m256i*)P);
        Signm = img.cols - A;
    }
    uchar* Ptr;
    __m256i fullSum = _mm256_setzero_si256();
    for (size_t row = 0; row < img.rows; ++row)
    {
        __m256i rowSum = _mm256_setzero_si256();
        Ptr = img.ptr<uchar>(row);
        for (size_t col = 0; col < bodyWidth; col += A)
        {
            const __m256i src_ = _mm256_loadu_si256((__m256i*)(Ptr + col));
            rowSum = _mm256_add_epi32(rowSum, Square8u(src_));
        }
        if (EndType)
        {
            const __m256i src_ = _mm256_and_si256(tailMask, _mm256_loadu_si256((__m256i*)(Ptr + Signm)));
            rowSum = _mm256_add_epi32(rowSum, Square8u(src_));
        }
        fullSum = _mm256_add_epi64(fullSum, HorizontalSum32(rowSum));
    }
    ResVal = 0;
    for (size_t i = 0; i < 4; i++)
    {
        ResVal += fullSum.m256i_i64[i];
    }
}

void ValueSquareSum(cv::Mat& img, uint64& ValueVal, uint64& SquareVal)
{
    static int A = 32;//AVX2
    assert(img.cols >= A);
    size_t bodyWidth = AlignLo(img.cols, A);
    bool EndType = bodyWidth != img.cols;
    size_t Signm = 0;

    __m256i tailMask;
    if (EndType)
    {
        int position = A - img.cols + bodyWidth;
        static uchar P[32] = { 0 };
        for (size_t i = 0; i < position; ++i)
            P[i] = 0;
        for (size_t i = position; i < 32; ++i)
            P[i] = 0xFF;
        tailMask = _mm256_loadu_si256((__m256i*)P);
        Signm = img.cols - A;
    }
    uchar* Ptr;
    __m256i fullValueSum = _mm256_setzero_si256();
    __m256i fullSquareSum = _mm256_setzero_si256();

    for (size_t row = 0; row < img.rows; ++row)
    {
        __m256i rowSquareSum = _mm256_setzero_si256();
        Ptr = img.ptr<uchar>(row);
        for (size_t col = 0; col < bodyWidth; col += A)
        {
            const __m256i value = _mm256_loadu_si256((__m256i*)(Ptr + col));
            fullValueSum = _mm256_add_epi64(_mm256_sad_epu8(value, K_ZERO), fullValueSum);
            rowSquareSum = _mm256_add_epi32(rowSquareSum, Square8u(value));
        }
        if (EndType)
        {
            const __m256i value = _mm256_and_si256(tailMask, _mm256_loadu_si256((__m256i*)(Ptr + Signm)));
            fullValueSum = _mm256_add_epi64(_mm256_sad_epu8(value, K_ZERO), fullValueSum);
            rowSquareSum = _mm256_add_epi32(rowSquareSum, Square8u(value));
        }
        fullSquareSum = _mm256_add_epi64(fullSquareSum, HorizontalSum32(rowSquareSum));
    }

    ValueVal = 0;
    SquareVal = 0;
    for (size_t i = 0; i < 4; i++)
    {
        ValueVal += fullValueSum.m256i_i64[i];
        SquareVal+= fullSquareSum.m256i_i64[i];
    }
}